#include <Wire.h>
#include "ADXL345.h"

#define TEMPO_ENTRE_LEITURAS       50
#define TEMPO_PARA_INICIALIZACAO 1500

// convencao de envio de sensores
#define SENSOR_ACELEROMETRO      "AC"
#define SENSOR_GIROSCOPIO        "GI"
#define SENSOR_BUSSOLA           "BU"
#define SENSOR_BAROMETRO         "BA"

// defines do giroscopio
#define CTRL_REG1                      0x20
#define CTRL_REG2                      0x21
#define CTRL_REG3                      0x22
#define CTRL_REG4                      0x23
#define CTRL_REG5                      0x24

// defines da bussola
#define HMC5883_WriteAddress           0x1E 
#define HMC5883_ModeRegisterAddress    0x02
#define HMC5883_ContinuousModeCommand  0x00
#define HMC5883_DataOutputXMSBAddress  0x03

// defines do barometro
#define BMP085_ADDRESS                 0x77 

// instancia de acesso ao acelerometro
ADXL345 adxl; 

// variaveis compartilhadas pelos sensores
int x,y,z; 

// endereco de acesso do giroscopio
const int L3G4200D_Address = 0x69;
// constantes de acesso da bussola
const int regb             = 0x01;
const int regbdata         = 0x40;
// constantes de acesso do barometro
const unsigned char OSS    = 0; 

///////////////////////////////////////////////////////// ACELEROMETRO

void prepararAcelerometro(){
 adxl.powerOn();

 adxl.setActivityThreshold(75); 
 adxl.setInactivityThreshold(75); 
 adxl.setTimeInactivity(10); 
 
 adxl.setActivityX(1);
 adxl.setActivityY(1);
 adxl.setActivityZ(1);

 adxl.setInactivityX(1);
 adxl.setInactivityY(1);
 adxl.setInactivityZ(1);

 adxl.setInterruptMapping( ADXL345_INT_ACTIVITY_BIT,   ADXL345_INT1_PIN );
 adxl.setInterruptMapping( ADXL345_INT_INACTIVITY_BIT,  ADXL345_INT1_PIN );

 adxl.setInterrupt( ADXL345_INT_ACTIVITY_BIT,  1);
 adxl.setInterrupt( ADXL345_INT_INACTIVITY_BIT, 1);
}

void lerAcelerometro(){
 adxl.readAccel(&x, &y, &z);
}

/////////////////////////////////////////////////////////// GIROSCOPIO

void prepararGiroscopio(){
  setupL3G4200D(2000); 
}

void lerGiroscopio(){
  getGyroValues();
}

void getGyroValues(){
  byte xMSB = readRegister(L3G4200D_Address, 0x29);
  byte xLSB = readRegister(L3G4200D_Address, 0x28);
  x = ((xMSB << 8) | xLSB);

  byte yMSB = readRegister(L3G4200D_Address, 0x2B);
  byte yLSB = readRegister(L3G4200D_Address, 0x2A);
  y = ((yMSB << 8) | yLSB);

  byte zMSB = readRegister(L3G4200D_Address, 0x2D);
  byte zLSB = readRegister(L3G4200D_Address, 0x2C);
  z = ((zMSB << 8) | zLSB);
}

int setupL3G4200D(int scale){
  writeRegister(L3G4200D_Address, CTRL_REG1, 0b00001111);
  writeRegister(L3G4200D_Address, CTRL_REG2, 0b00000000);
  writeRegister(L3G4200D_Address, CTRL_REG3, 0b00001000);
  if(scale == 250){
    writeRegister(L3G4200D_Address, CTRL_REG4, 0b00000000);
  }else if(scale == 500){
    writeRegister(L3G4200D_Address, CTRL_REG4, 0b00010000);
  }else{
    writeRegister(L3G4200D_Address, CTRL_REG4, 0b00110000);
  }
  writeRegister(L3G4200D_Address, CTRL_REG5, 0b00000000);
}

int readRegister(int deviceAddress, byte address){
    int v;
    Wire.beginTransmission(deviceAddress);
    Wire.write(address); 
    Wire.endTransmission();
    Wire.requestFrom(deviceAddress, 1); 
    while(!Wire.available()) {
        // waiting
    }

    v = Wire.read();
    return v;
}

///////////////////////////////////////////////////////////// BUSSOLA

void lerBussola() {
    int outputData[6];
    int i;
 
    Wire.beginTransmission(HMC5883_WriteAddress);
    Wire.write(regb);
    Wire.write(regbdata);
    Wire.endTransmission();
    delay(100);
 
    Wire.beginTransmission(HMC5883_WriteAddress);
    Wire.write(HMC5883_ModeRegisterAddress);      
    Wire.write(HMC5883_ContinuousModeCommand);     
    Wire.endTransmission();                      
    delay(100);
 
 
    Wire.beginTransmission(HMC5883_WriteAddress);  
    Wire.requestFrom(HMC5883_WriteAddress,6);     
 
    delay(100);
 
    if(6 <= Wire.available())
    {
        for(i=0;i<6;i++)
        {
            outputData[i]=Wire.read();
        }
    }
    x=outputData[0] << 8 | outputData[1]; 
    z=outputData[2] << 8 | outputData[3]; 
    y=outputData[4] << 8 | outputData[5];
}

/////////////////////////////////////////////////////////// BAROMETRO

int ac1;
int ac2;
int ac3;
unsigned int ac4;
unsigned int ac5;
unsigned int ac6;
int b1;
int b2;
int mb;
int mc;
int md;

long b5; 

void prepararBarometro(){
  bmp085Calibration();
}

void lerBarometro()
{
  x = bmp085GetTemperature(bmp085ReadUT());
  y = bmp085GetPressure(bmp085ReadUP());
  z = 0;
}

void bmp085Calibration()
{
  ac1 = bmp085ReadInt(0xAA);
  ac2 = bmp085ReadInt(0xAC);
  ac3 = bmp085ReadInt(0xAE);
  ac4 = bmp085ReadInt(0xB0);
  ac5 = bmp085ReadInt(0xB2);
  ac6 = bmp085ReadInt(0xB4);
  b1 = bmp085ReadInt(0xB6);
  b2 = bmp085ReadInt(0xB8);
  mb = bmp085ReadInt(0xBA);
  mc = bmp085ReadInt(0xBC);
  md = bmp085ReadInt(0xBE);
}

float bmp085GetTemperature(unsigned int ut){
  long x1, x2;

  x1 = (((long)ut - (long)ac6)*(long)ac5) >> 15;
  x2 = ((long)mc << 11)/(x1 + md);
  b5 = x1 + x2;

  float temp = ((b5 + 8)>>4);
  temp = temp /10;

  return temp;
}

long bmp085GetPressure(unsigned long up){
  long x1, x2, x3, b3, b6, p;
  unsigned long b4, b7;

  b6 = b5 - 4000;
  // Calculate B3
  x1 = (b2 * (b6 * b6)>>12)>>11;
  x2 = (ac2 * b6)>>11;
  x3 = x1 + x2;
  b3 = (((((long)ac1)*4 + x3)<<OSS) + 2)>>2;

  // Calculate B4
  x1 = (ac3 * b6)>>13;
  x2 = (b1 * ((b6 * b6)>>12))>>16;
  x3 = ((x1 + x2) + 2)>>2;
  b4 = (ac4 * (unsigned long)(x3 + 32768))>>15;

  b7 = ((unsigned long)(up - b3) * (50000>>OSS));
  if (b7 < 0x80000000)
    p = (b7<<1)/b4;
  else
    p = (b7/b4)<<1;

  x1 = (p>>8) * (p>>8);
  x1 = (x1 * 3038)>>16;
  x2 = (-7357 * p)>>16;
  p += (x1 + x2 + 3791)>>4;

  long temp = p;
  return temp;
}

char bmp085Read(unsigned char address)
{
  unsigned char data;

  Wire.beginTransmission(BMP085_ADDRESS);
  Wire.write(address);
  Wire.endTransmission();

  Wire.requestFrom(BMP085_ADDRESS, 1);
  while(!Wire.available())
    ;

  return Wire.read();
}

int bmp085ReadInt(unsigned char address)
{
  unsigned char msb, lsb;

  Wire.beginTransmission(BMP085_ADDRESS);
  Wire.write(address);
  Wire.endTransmission();

  Wire.requestFrom(BMP085_ADDRESS, 2);
  while(Wire.available()<2)
    ;
  msb = Wire.read();
  lsb = Wire.read();

  return (int) msb<<8 | lsb;
}

unsigned int bmp085ReadUT(){
  unsigned int ut;

  Wire.beginTransmission(BMP085_ADDRESS);
  Wire.write(0xF4);
  Wire.write(0x2E);
  Wire.endTransmission();

  delay(5);

  ut = bmp085ReadInt(0xF6);
  return ut;
}

unsigned long bmp085ReadUP(){

  unsigned char msb, lsb, xlsb;
  unsigned long up = 0;

  Wire.beginTransmission(BMP085_ADDRESS);
  Wire.write(0xF4);
  Wire.write(0x34 + (OSS<<6));
  Wire.endTransmission();

  delay(2 + (3<<OSS));

  msb = bmp085Read(0xF6);
  lsb = bmp085Read(0xF7);
  xlsb = bmp085Read(0xF8);

  up = (((unsigned long) msb << 16) | ((unsigned long) lsb << 8) | (unsigned long) xlsb) >> (8-OSS);

  return up;
}

/////////////////////////////////////////////////////////////////////

void writeRegister(int deviceAddress, byte address, byte val) {
    Wire.beginTransmission(deviceAddress);  
    Wire.write(address);  
    Wire.write(val);  
    Wire.endTransmission();
}

void enviarEixos(String sensor) {
  Serial.print("<" + sensor + ">");
  Serial.print(x);
  Serial.print("|");
  Serial.print(y);
  Serial.print("|");
  Serial.print(z); 
  Serial.print("</" + sensor + ">");
}

void setup() {
  Wire.begin();
  Serial.begin(9600);

  prepararAcelerometro();
  prepararGiroscopio();
  prepararBarometro();

  delay(TEMPO_PARA_INICIALIZACAO);  
}

void loop() {
 lerAcelerometro();
 enviarEixos(SENSOR_ACELEROMETRO); // enviar os dados do acelerometro pela usb
 lerGiroscopio();
 enviarEixos(SENSOR_GIROSCOPIO);
 lerBussola();
 enviarEixos(SENSOR_BUSSOLA);
 lerBarometro();
 enviarEixos(SENSOR_BAROMETRO);
 
 delay(TEMPO_ENTRE_LEITURAS);
}
